;;;; -*- Mode: Lisp; -*-


(in-package :lib.gl)

(defun make-vector (&rest values)
  (case *default-matrix-constructor*
    (external-array (apply #'%external-vector values))
    (array (apply #'vector values))))

(defmethod mref ((v vector) row &optional col)
  (declare (ignore col))
  (aref v row))

(defmethod (setf mref) (val (v vector) &rest indices)
   (setf (aref v (car indices)) val))

(defmethod vector-length ((vec vector))
  (length vec))

(defmethod element-type ((vec vector))
  (type-of (aref vec 0)))

(defmethod element-count ((vec vector))
  (vector-length vec))

(defun make-column-vector (elems)
  (make-vector elems))

(defun vector2-p (v)
  (= (vector-length v) 2))

(defun vector3-p (v)
  (= (vector-length v) 3))

(defmethod copy-vector ((v vector))
  (let ((vn (zero-vector (vector-length v))))
    (iter (for vi in-vector v)
	  (for i from 0)
	  (setf (mref vn i) vi))
    vn))

;;; Kind of breaking the abstraction layer by doing this,
;;; should have a general purpose make-vector that accepts a
;;; dimension and initial element.
(defun zero-vector (size &key (element-type 'single-float))
  (make-array size :initial-element (coerce +zero+ element-type)))

(defun ones-vector (size &key (element-type 'single-float))
  (make-array size :initial-element (coerce +one+ element-type)))

(defun unit-x-vector (&optional (dim 3))
  (cond ((= dim 2) (vector +one+ +zero+))
        ((= dim 3) (vector +one+ +zero+ +zero+))
        (t (vector +one+ +zero+ +zero+ +zero+))))

(defun unit-y-vector (&optional (dim 3))
  (cond ((= dim 2) (vector +zero+ +one+))
        ((= dim 3) (vector +zero+ +one+ +zero+))
        (t (vector +zero+ +one+ +zero+ +zero+))))

(defun unit-z-vector (&optional (dim 3))
  (cond ((= dim 3) (vector +zero+ +zero+ +one+))
        (t (vector +zero+ +zero+ +one+ +zero+))))

(defun unit-w-vector ()
  (vector +zero+ +zero+ +zero+ +one+))

(defun vector= (v1 v2)
  (if (and (= (vector-length v1) (vector-length v2))
           (iter (for x in-vector v1)
                 (for y in-vector v2)
                 (always (= x y))))
      t
    nil))

(defun vector-plus-p (v)
  (iter (for x in-vector v)
	(always (plusp x))))

(defun vector-neg-p (v)
  (not (vector-plus-p v)))

;;; Convenience accessors for vectors in 2-D and 3-D coordinate systems.
(defun x (v)
  (if (typep v 'quaternion)
       (mref v 1)
     (mref v 0)))

(defun set-x (v new-value)
  (if (typep v 'quaternion)
      (setf (mref v 1) new-value)
    (setf (mref v 0) new-value)))

(defun y (v)
  (if (typep v 'quaternion)
      (mref v 2)
    (mref v 1)))

(defun set-y (v new-value)
  (if (typep v 'quaternion)
      (setf (mref v 2) new-value)
    (setf (mref v 1) new-value)))

(defun z (v)
  (if (typep v 'quaternion)
      (mref v 3)
    (mref v 2)))

(defun set-z (v new-value)
  (if (typep v 'quaternion)
      (setf (mref v 3) new-value)
    (setf (mref v 2) new-value)))

(defsetf x set-x)
(defsetf y set-y)
(defsetf z set-z)

;;; Vector arithmatic
(defun %mult-scalar-vec (k v)
  (let ((rv (make-array (vector-length v)))) ; :element-type (element-type v))))
    (dotimes (i (vector-length v) rv)
      (setf (mref rv i) (* k (mref v i))))))

(defun %div-scalar-vec (k v)
  (let ((rv (make-array (vector-length v))) ; :element-type (element-type v)))
        (ik (if (= 0 k)
                least-positive-double-float
		(/ 1.0 k))))
    (dotimes (i (vector-length v) rv)
      (setf (mref rv i) (* ik (mref v i))))))

;      (let ((fac (* ik (mref v i))))
;	(handler-case (setf (mref rv i) fac)
;	  (ccl::array-element-type-error () (let ((etype (type-of (mref rv i))))
;					      (setf (mref rv i ) (coerce fac etype)))))))))

(defun %add-vec (v1 v2)
  (unless (= (vector-length v1) (vector-length v2))
    (error "Vector ~A is not of the same size (~A) as ~A (~A).  Check your math!" 
           v1 (vector-length v1) v2 (vector-length v2)))
  (let ((vec (make-array (vector-length v1)))); :element-type (element-type v1))))
    (dotimes (i (vector-length v1) vec)
      (setf (mref vec i) (+ (mref v1 i) (mref v2 i))))))

(defun %sub-vec (v1 v2 &optional (v3 nil))
  (unless (= (vector-length v1) (vector-length v2))
    (error "Vector ~A is not of the same size (~A) as ~A (~A).  Check your math!" 
           v1 (vector-length v1) v2 (vector-length v2)))
  (let ((vec (or v3 (make-array (vector-length v1))))); :element-type (element-type v1))))
    (dotimes (i (vector-length v1) vec)
      (setf (mref vec i) (- (mref v1 i) (mref v2 i))))))

(defun %mult-vec-vec (v1 v2)
  (unless (= (vector-length v1) (vector-length v2))
    (error "Vector ~A is not of the same size (~A) as ~A (~A).  Check your math!" 
           v1 (vector-length v1) v2 (vector-length v2)))
  (let* ((i_n (vector-length v1))
         (vec (make-array i_n))); :element-type (element-type v1))))
    (dotimes (i i_n vec)
      (setf (mref vec i) (* (mref v1 i) (mref v2 i))))))

(defmethod m+ ((v1 vector) (v2 vector) &rest args)
  (if (not (null args))
      (apply #'m+ (%add-vec v1 v2) (first args) (rest args))
    (%add-vec v1 v2)))

(defmethod m- ((v1 vector) (v2 vector) &rest args)
  (if (not (null args))
      (apply #'m- (%sub-vec v1 v2) (first args) (rest args))
    (%sub-vec v1 v2)))

(defmethod m* ((k number) (q vector) &rest rest)
  (if (not (null rest))
      (apply #'m* (%mult-scalar-vec k q) (first rest) (rest rest))
    (%mult-scalar-vec k q)))

(defmethod m* ((q vector) (u vector) &rest rest)
  (if (not (null rest))
      (apply #'m* (%mult-vec-vec q u) (first rest) (rest rest))
    (%mult-vec-vec q u)))

(defmethod m/ ((q vector) (k number) &rest rest)
  (if (not (null rest))
      (apply #'m* (%div-scalar-vec k q) (first rest) (rest rest))
    (%div-scalar-vec k q)))

(defmethod mneg ((v vector))
  (let* ((l (vector-length v))
	 (nvec (make-array l :initial-element (mref v 0))))
    (dotimes (i l nvec)
      (setf (mref nvec i) (- (mref v i))))))

;;; This is length in WildMagic!
(defmethod magnitude ((v vector))
  (sqrt (iter (for n in-vector v)
              (summing (* n n)))))

(defmethod squared-magnitude ((v vector))
  (iter (for n in-vector v)
        (summing (* n n))))

(defmethod dot-product ((v1 vector) (v2 vector))
  (iter (for v1i in-vector v1)
   	(for v2i in-vector v2)
   	(summing (* v1i v2i))))

(defun dot (v1 v2)
  (dot-product v1 v2))

(defun perp (v)
  (make-vector (mref v 1) (- (mref v 0))))


(defmethod normalize ((v vector))
  "Returns the normalized vector for v.  As a side-effect will also convert v to a normalized vector."
  ;;; In WildMagic this returns the length but not v, why??? C++ is weird.
  (let ((mag (magnitude v)))
    (if (> mag +epsilon+)
        (let ((imag (/ 1 mag)))
          (dotimes (i (vector-length v) v)
            (setf (mref v i) (* imag (mref v i)))))
      (dotimes (i (vector-length v) v)
        (setf (mref v i) +zero+))))) 
 
(defmethod cross ((u vector) (v vector))
  (make-vector (- (* (mref u 1) (mref v 2)) (* (mref u 2) (mref v 1)))
               (- (* (mref u 2) (mref v 0)) (* (mref u 0) (mref v 2)))
               (- (* (mref u 0) (mref v 1)) (* (mref u 1) (mref v 0)))))

(defmethod unit-cross ((v1 vector) (v2 vector))
  (normalize (cross v1 v2)))

(defmethod orthonormalize ((u vector) (v vector) (w vector))
  "Destructive modifies u, v, and w to produce a set of orthonormal vectors."
  (let* ((u0 (normalize u))
         (u1 (normalize (m- v (m* (dot-product u0 v) u0))))
         (dot0 (dot-product u0 w))
         (dot1 (dot-product u1 w))
         (u2 (normalize (m- w (m+ (m* dot0 u0)
                                  (m* dot1 u1))))))
    (setf u u0 v u1 w u2)
    (values u0 u1 u2)))

(defmethod generate-orthonormal-basis ((u vector) (v vector) (w vector) &key (unit-length-w-p nil))
  "Decstructively modifies u, v, and w to produce an orthonormal basis of the vector space described
by the vectors."
  (unless unit-length-w-p
    (normalize w))
  (if (>= (abs (x w))
          (abs (y w)))
      (let ((ilength (/ 1.0 (sqrt (+ (* (x w) (x w)) (* (z w) (z w)))))))
        (setf (mref u 0) (* -1 (mref w 2) ilength)
              (mref u 1) +zero+
              (mref u 2) (* (mref w 0) ilength)))
    (let ((ilength (/ 1.0 (sqrt (+ (* (y w) (y w)) (* (z w) (z w)))))))
      (setf (mref u 0) +zero+
            (mref u 1) (* (mref w 2) ilength)
            (mref u 2) (* -1 (mref w 2) ilength))))
  (setf v (cross w u))
  (values u v w))

(defmethod get-barycentrics ((vec vector) rkv0 rkv1 rkv2 rkv3)
  ;;;; Points are vectors
  (let ((akdiff (make-matrix 4 3 :initial-element +zero+))
        (afbary (zero-vector 4))
        fmax fvalue finvmax fdet ke01 ke1ce2 ke2ce0 ke0ce1 finvdet ke02 ke12 ke02ce12
        fmaxsqrarea fsqrarea finvsqrarea imaxindex fsqrlength finvsqrlength fmaxsqrlength ktmp)
    (set-column akdiff 0 (m- rkv0 rkv3))
    (set-column akdiff 1 (m- rkv1 rkv3))
    (set-column akdiff 2 (m- rkv2 rkv3))
    (set-column akdiff 3 (m- vec rkv3))
    ;; If the vertices have large magnitude, the linear system of
    ;; equations for computing barycentric coordinates can be
    ;; ill-conditioned.  To avoid this, uniformly scale the tetrahedron
    ;; edges to be of order 1.  The scaling of all differences does not
    ;; change the barycentric coordinates.    
    (setf fmax +zero+)
    (do ((i 0 (incf i)))
        ((= i 3))
      (do ((j 0 (incf j)))
          ((= j 3))  
        (setf fvalue (abs (mref akdiff i j)))
        (if (> fvalue fmax)
            (setf fmax fvalue))))
    (if (> fmax 1.0) 
        (progn
          (setf finvmax (/ 1.0 fmax))
          (do ((i 0 (incf i)))
              ((= i 4))
            (set-column akdiff i (m* (get-column akdiff i) finvmax)))))
    (setf fdet (dot-product (get-column akdiff 0) (cross (get-column akdiff 1) (get-column akdiff 2))))
    (setf ke1ce2 (cross (get-column akdiff 1) (get-column akdiff 2)))
    (setf ke2ce0 (cross (get-column akdiff 2) (get-column akdiff 0)))
    (setf ke0ce1 (cross (get-column akdiff 0) (get-column akdiff 1)))
    (if (> (abs fdet) +epsilon+)
        (progn
          (setf finvdet (/ 1.0 fdet))
          (setf (mref afbary 0) (* (dot-product (get-column akdiff 3) ke1ce2) finvdet))
          (setf (mref afbary 1) (* (dot-product (get-column akdiff 3) ke2ce0) finvdet))
          (setf (mref afbary 2) (* (dot-product (get-column akdiff 3) ke0ce1) finvdet))
          (setf (mref afbary 3) (- (- (- 1.0 (mref afbary 0)) (mref afbary 1)) (mref afbary 2))))
      ;; The tetrahedron is potentially flat.  Determine the face of
      ;; maximum area and compute barycentric coordinates with respect
      ;; to that face.
      (progn
        (setf ke02 (m- rkv0 rkv2))
        (setf ke12 (m- rkv1 rkv2))
        (setf ke02ce12 (cross ke02 ke12))
        (setf fmaxsqrarea (squared-magnitude ke02ce12))
        (setf imaxindex 3)
        (setf fsqrarea (squared-magnitude ke0ce1))
        (if (> fsqrarea fmaxsqrarea) 
            (progn (setf imaxindex 0) (setf fmaxsqrarea fsqrarea)))
        (setf fsqrarea (squared-magnitude ke1ce2))
        (if (> fsqrarea fmaxsqrarea) (progn (setf imaxindex 1) (setf fmaxsqrarea fsqrarea)))
        (setf fsqrarea (squared-magnitude ke2ce0))
        (if (> fsqrarea fmaxsqrarea) (progn (setf imaxindex 2) (setf fmaxsqrarea fsqrarea)))
        (if (> fmaxsqrarea +epsilon+)
            (progn
              (setf finvsqrarea (/ +one+ fmaxsqrarea))
              (if (= imaxindex 0)
                  (progn
                    (setf ktmp (cross (get-column akdiff 3) (get-column akdiff 1)))
                    (setf (mref afbary 0) (* (dot-product ke0ce1 ktmp) finvsqrarea))
                    (setf ktmp (cross (get-column akdiff 0) (get-column akdiff 3)))
                    (setf (mref afbary 1) (* (dot-product ke0ce1 ktmp) finvsqrarea))
                    (setf (mref afbary 2) +zero+)
                    (setf (mref afbary 3) (- (- +one+ (mref afbary 0)) (mref afbary 1))))
                (if (= imaxindex 1)
                    (progn
                      (setf (mref afbary 0) +zero+)
                      (setf ktmp (cross (get-column akdiff 3) (get-column akdiff 2)))
                      (setf (mref afbary 1) (* (dot ke1ce2 ktmp) finvsqrarea))
                      (setf ktmp (cross (get-column akdiff 1) (get-column akdiff 3)))
                      (setf (mref afbary 2) (* (dot ke1ce2 ktmp) finvsqrarea))
                      (setf (mref afbary 3) (- (- +one+ (mref afbary 1)) (mref afbary 2))))
                  (if (= imaxindex 2)
                      (progn
                        (setf ktmp (cross (get-column akdiff 2) (get-column akdiff 3)))
                        (setf (mref afbary 0) (* (dot ke2ce0 ktmp) finvsqrarea))
                        (setf (mref afbary 1) +zero+)
                        (setf ktmp (cross (get-column akdiff 3) (get-column akdiff 0)))
                        (setf (mref afbary 2) (* (dot ke2ce0 ktmp) finvsqrarea))
                        (setf (mref afbary 3) (- (- +one+ (mref afbary 0)) (mref afbary 2))))
                    (progn
                      (set-column akdiff 3 (m- vec rkv2))
                      (setf ktmp (cross (get-column akdiff 3) ke12))
                      (setf (mref afbary 0) (* (dot ke02ce12 ktmp) finvsqrarea))
                      (setf ktmp (cross ke02 (get-column akdiff 3)))
                      (setf (mref afbary 1) (* (dot ke02ce12 ktmp) finvsqrarea))
                      (setf (mref afbary 2) (- (- +one+ (mref afbary 0)) (mref afbary 1)))
                      (setf (mref afbary 3) +zero+))))))
          ;; The tetrahedron is potentially a sliver.  Determine the edge of
          ;; maximum length and compute barycentric coordinates with respect
          ;; to that edge.
          (progn
            (setf fmaxsqrlength (squared-magnitude (get-column akdiff 0)))
            (setf imaxindex 0)
            (setf fsqrlength (squared-magnitude (get-column akdiff 1)))
            (if (> fsqrlength fmaxsqrlength) 
                (progn (setf imaxindex 1) (setf fmaxsqrlength fsqrlength)))
            (setf fsqrlength (squared-magnitude (get-column akdiff 2)))
            (if (> fsqrlength fmaxsqrlength) 
                (progn (setf imaxindex 2) (setf fmaxsqrlength fsqrlength)))
            (setf fsqrlength (squared-magnitude ke02))
            (if (> fsqrlength fmaxsqrlength) 
                (progn (setf imaxindex 3) (setf fmaxsqrlength fsqrlength)))
            (setf fsqrlength (squared-magnitude ke12))
            (if (> fsqrlength fmaxsqrlength) 
                (progn (setf imaxindex 4) (setf fmaxsqrlength fsqrlength)))
            (setf ke01 (m- rkv0 rkv1))
            (setf fsqrlength (squared-magnitude ke01))
            (if (> fsqrlength fmaxsqrlength) 
                (progn (setf imaxindex 5) (setf fmaxsqrlength fsqrlength)))
            (if (> fmaxsqrlength +epsilon+)
                (progn
                  (setf finvsqrlength (/ 1.0 fmaxsqrlength))
                  (if (= imaxindex 0)
                      (progn
                        (setf (mref afbary 0) (* (dot (get-column akdiff 3) (get-column akdiff 0)) finvsqrlength))
                        (setf (mref afbary 1) +zero+)
                        (setf (mref afbary 2) +zero+)
                        (setf (mref afbary 3) (- +one+ (mref afbary 0))))
                    (if (= imaxindex 1)
                        (progn
                          (setf (mref afbary 0) +zero+)
                          (setf (mref afbary 1) (* (dot (get-column akdiff 3) (get-column akdiff 1)) finvsqrlength))
                          (setf (mref afbary 2) +zero+)
                          (setf (mref afbary 3) (- +one+ (mref afbary 1))))
                      (if (= imaxindex 2)
                          (progn
                            (setf (mref afbary 0) +zero+)
                            (setf (mref afbary 1) +zero+)
                            (setf (mref afbary 2) (* (dot (get-column akdiff 3) (get-column akdiff 2)) finvsqrlength))
                            (setf (mref afbary 3) (- +one+ (mref afbary 2))))
                        (if (= imaxindex 3)
                            (progn
                              (set-column akdiff 3 (m- vec rkv2))
                              (setf (mref afbary 0) (* (dot (get-column akdiff 3) ke02) finvsqrlength))
                              (setf (mref afbary 1) +zero+)
                              (setf (mref afbary 2) (- +one+ (mref afbary 0)))
                              (setf (mref afbary 3) +zero+))
                          (if (= imaxindex 4)
                              (progn
                                (set-column akdiff 3 (m- vec rkv2))
                                (setf (mref afbary 0) +zero+)
                                (setf (mref afbary 1) (* (dot (get-column akdiff 3) ke12) finvsqrlength))
                                (setf (mref afbary 2) (- +one+ (mref afbary 1)))
                                (setf (mref afbary 3) +zero+))
                            (progn
                              (set-column akdiff 3 (m- vec rkv1))
                              (setf (mref afbary 0) (* (dot (get-column akdiff 3) ke01) finvsqrlength))
                              (setf (mref afbary 1) (- +one+ (mref afbary 0)))
                              (setf (mref afbary 2) +zero+)
                              (setf (mref afbary 3) +zero+))))))))
              (progn
                ;; tetrahedron is a nearly a point, just return equal weights
                (setf (mref afbary 0) 0.25d0)
                (setf (mref afbary 1) (mref afbary 0))
                (setf (mref afbary 2) (mref afbary 0))
                (setf (mref afbary 3) (mref afbary 0))))))))
    afbary))

(defmethod affine-sum ((k number) (v1 vector) (v2 vector))
  (let ((vec (make-array (length v1) :initial-element +zero+)))
    (dotimes (i (length v1) vec)
      (setf (mref vec i) (+ (mref v1 i)
                             (* k (- (mref v2 i) (mref v1 i))))))))


